#include "bldc_communicate.h"
const char BLDC_CommandRun[]       = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc};
const char BLDC_CommandStop[]      = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfd};
const char BLDC_CommandSetOrigin[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe};
extern BLDC_Device        *BLDC_List[3];
BLDC_CAN_RecieveDataFrame CANRecieveBuffer;
int recieve_num;
int CANRecieveCompleted;
int BLDC_ParameterSend(BLDC_Device *device, const char *param)
{
    BLDC_CAN_SendDataFrame buffer;
    if(param == NULL)
    {
        buffer[0]  = device->ref_position >> 8;
        buffer[0] |= device->ref_position << 8;
        buffer[1]  = device->ref_speed  << 4  ;
        buffer[1] |= device->PD_Kp  >> 8      ;
        buffer[2]  = device->PD_Kp  << 8      ;
        buffer[2] |= device->PD_Kd  >> 4      ;
        buffer[3]  = device->PD_Kd  << 12     ;
        buffer[3] |= device->ref_moment       ;
    }
    else
    {
        memcpy(&buffer, param, 8);
    }
    _USART_CAN_SENDDATA(device);  //Change to use dma
    return ERROR_OK;
}

int USART_CAN_SendData(USART_Device *device, uint16_t ID, void *data, int size)
{
        uint8_t * id = (uint8_t*)(&ID);
        uint8_t * send_data =(uint8_t *)data;
        _USART_SENDDATA(id[1]);
        while(!USART_GetFlagStatus(device->USART_Base, USART_FLAG_TXC));
        _USART_SENDDATA(id[0]);
        for(int i = 0; i < size; i++)
        {
            while(!USART_GetFlagStatus(device->USART_Base, USART_FLAG_TXC));
            _USART_SENDDATA(send_data[i]);
        }
        return ERROR_OK;
}

void USART0_IRQHandler(void)
{
    int Timeout = 0x1FFE;
    if(recieve_num < CAN_USART_RECESIZE){
        while(recieve_num < CAN_USART_RECESIZE && Timeout){
            while(--Timeout && (!USART_GetFlagStatus(HT_USART0, USART_FLAG_RXDR)));
            if(Timeout){
                CANRecieveBuffer[recieve_num++] = USART_ReceiveData(HT_USART0);
            }
        }
    }
    if(recieve_num == CAN_USART_RECESIZE){
        CANRecieveCompleted = 1;
    }
    else
    {
        CANRecieveCompleted = 0;
    }
	recieve_num = 0;
    NVIC_ClearPendingIRQ(USART0_IRQn);
}

int CAN_RecieveDataHandler(BLDC_CAN_RecieveDataFrame RecieveBuffer, BLDC_Device **device_list)
{
    uint16_t CAN_ID, temp, BLDC_ID = 0;
    BLDC_Device *device;
    CAN_ID = RecieveBuffer[1];
    if(CAN_ID != CAN_MASTER_ID)
    {
        return ERROR_CAN_MASTER_ID_DISMATCH;
    }
    else
    {
        CAN_ID = RecieveBuffer[2];
    }
    while(*(device_list + BLDC_ID) != NULL)
    {
        if(CAN_ID == (*device_list[BLDC_ID]).ID)
            break;
        else
            ++BLDC_ID;
    }
    if((device = *(device_list + BLDC_ID)) == NULL)
    {
        return ERROR_COULDNT_MATCH_DEVICE;
    }
    else
    {
        /*posiition*/
        temp  = RecieveBuffer[3];
        temp <<= 8;
        temp |= RecieveBuffer[4];
        device->position = temp;
        /*speed*/
        temp  = RecieveBuffer[5];
        temp <<= 4;
        temp |= (RecieveBuffer[6] >> 4);
        device->speed = temp;
        /*moment*/
        temp  = (RecieveBuffer[6] << 4);
        temp <<= 4;
        temp |= RecieveBuffer[7];
        device->moment = temp;
        
        return ERROR_OK;
    }
}

void PDMA_CH2_5_IRQHandler(void)
{
    CAN_RecieveDataHandler(CANRecieveBuffer, BLDC_List);
    HT_PDMA->PDMACH2.DADR = (unsigned int)(CANRecieveBuffer);
    PDMA_ClearFlag(PDMA_CH2, PDMA_FLAG_GE | PDMA_FLAG_TC);
}

void BLDC_RS485_SendCommand(char ch, BLDC_Device *device)
{
    USART_SendData(device->RS485_device->USART_Base, ch);
    USART_SendData(device->RS485_device->USART_Base, '\r');
    USART_SendData(device->RS485_device->USART_Base, '\n');
}
